1 #!/usr/bin/env python
   2 
   3 import roslib; roslib.load_manifest('smach_tutorials')
   4 import rospy
   5 import smach
   6 import smach_ros
   7 
   8 from smach_tutorials.msg import TestAction, TestGoal
   9 from actionlib import *
  10 from actionlib_msgs.msg import *
  11 
  12 
  13 # Create a trivial action server
  14 class TestServer:
  15     def __init__(self,name):
  16         self._sas = SimpleActionServer(name,
  17                 TestAction,
  18                 execute_cb=self.execute_cb)
  19 
  20     def execute_cb(self, msg):
  21         if msg.goal == 0:
  22             self._sas.set_succeeded()
  23         elif msg.goal == 1:
  24             self._sas.set_aborted()
  25         elif msg.goal == 2:
  26             self._sas.set_preempted()
  27 
  28 def main():
  29     rospy.init_node('smach_example_actionlib')
  30 
  31     # Start an action server
  32     server = TestServer('test_action')
  33 
  34     # Create a SMACH state machine
  35     sm0 = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])
  36 
  37     # Open the container
  38     with sm0:
  39         # Add states to the container
  40 
  41         # Add a simple action state. This will use an empty, default goal
  42         # As seen in TestServer above, an empty goal will always return with
  43         # GoalStatus.SUCCEEDED, causing this simple action state to return
  44         # the outcome 'succeeded'
  45         smach.StateMachine.add('GOAL_DEFAULT',
  46                                smach_ros.SimpleActionState('test_action', TestAction),
  47                                {'succeeded':'GOAL_STATIC'})
  48 
  49         # Add another simple action state. This will give a goal
  50         # that should abort the action state when it is received, so we
  51         # map 'aborted' for this state onto 'succeeded' for the state machine.
  52         smach.StateMachine.add('GOAL_STATIC',
  53                                smach_ros.SimpleActionState('test_action', TestAction,
  54                                                        goal = TestGoal(goal=1)),
  55                                {'aborted':'GOAL_CB'})
  56 
  57         
  58         # Add another simple action state. This will give a goal
  59         # that should abort the action state when it is received, so we
  60         # map 'aborted' for this state onto 'succeeded' for the state machine.
  61         def goal_callback(userdata, default_goal):
  62             goal = TestGoal()
  63             goal.goal = 2
  64             return goal
  65 
  66         smach.StateMachine.add('GOAL_CB',
  67                                smach_ros.SimpleActionState('test_action', TestAction,
  68                                                        goal_cb = goal_callback),
  69                                {'aborted':'succeeded'})
  70 
  71         # For more examples on how to set goals and process results, see 
  72         # executive_smach/smach_ros/tests/smach_actionlib.py
  73 
  74     # Execute SMACH plan
  75     outcome = sm0.execute()
  76 
  77     rospy.signal_shutdown('All done.')
  78 
  79 
  80 if __name__ == '__main__':
  81     main()